#include "rod.h"

#define THRESHOLD 1e-5

Rod::Rod(float totalMass, const QVector3D &center, QObject *parent) : Object3D(center,parent)
{
    dt = 0.005;
    int nAmostras = 21;
    float x0 = -1;
    float zm = -1;
    float xf =  1;
    dm = totalMass/nAmostras;

    QVector3D p1(x0, 0.0, 0.0);
    QVector3D p2((x0+xf)/2, 0.0, zm);
    QVector3D p3(xf, 0.0, 0.0);

    for(int i = 0; i < nAmostras; ++i)
    {
        float t = float(i)/(nAmostras-1);
        QVector3D p = (1-t)*(1-t)*p1 + (1-t)*(t)*p2 + t*t*p3;
        //QVector3D p = (1-t)*p1 + t*p3;
        posicoes.append(p);
        velocidades.append(QVector3D(0,0,0));
    }

    for(int i = 0; i < posicoes.size()-1; ++i)
    {
        l.append((posicoes[i+1]-posicoes[i]).length());
    }

}

void Rod::drawGeometry(void)
{
    glDisable(GL_LIGHTING);
    glLineWidth(3);
    glPointSize(6);

    /*glColor3f(1.0,1.0,1.0);
    glBegin(GL_LINES);
    glVertex3f(0.0,0.0,0.0);
    glVertex3f(G.x(),G.y(),G.z());
    glEnd();*/

    glColor3f(0.5,0.5,0.5);
    glBegin(GL_LINE_STRIP);
    for(int i = 0; i < posicoes.size(); ++i)
    {
        glVertex3f(posicoes[i].x() , posicoes[i].y(), posicoes[i].z());
    }
    glEnd();

    glColor3f(1.0,0.0,0.0);
    glBegin(GL_POINTS);
    for(int i = 0; i < posicoes.size(); ++i)
    {
        glVertex3f(posicoes[i].x() , posicoes[i].y(), posicoes[i].z());
    }
    glEnd();
    glEnable(GL_LIGHTING);
}

void Rod::temporalIntegration()
{
    G = (modelViewMatrix().inverted().transposed().inverted()) *QVector3D(0.0, -9.8, 0.0);

    for(int i = 1; i < velocidades.size()-1; ++i)
    {
        velocidades[i] = G*dt + velocidades[i];
    }

    fastManifoldProjectionVelocityFilter();

    for(int i = 1; i < posicoes.size()-1; ++i)
    {
        posicoes[i] = velocidades[i]*dt + posicoes[i];
    }
}

QVector<QVector3D> Rod::fromVectorToQVector3D(const Vector& v)
{
    QVector<QVector3D> x;
    x.append(posicoes[0]);
    for(int i = 0; i < v.dim()/3; ++i)
        x.append(QVector3D(v(i*3 + 0), v(i*3 + 1), v(i*3 + 2)));
    x.append(posicoes.last());
    return x;
}

float sunLength(const Vector& X)
{
    double s = 0;
    for(int i = 0; i < X.dim(); ++i)
        s += X(i)*X(i);
    return s;
}

void Rod::fastManifoldProjectionVelocityFilter()
{
    float invDm = 1.0/dm;

    Vector X((posicoes.size()-2)*3);
    for(int i = 0; i < X.dim()/3; ++i)
    {
        QVector3D v = posicoes[i+1] + velocidades[i+1]*dt;
        X(3*i+0) = v.x();
        X(3*i+1) = v.y();
        X(3*i+2) = v.z();
    }

    QVector<QVector3D> Xt = fromVectorToQVector3D(X);
    Vector C = getC(Xt);

    while (sunLength(C) > THRESHOLD)
    {

        Matrix gradC = getGradC(Xt);
        Matrix A = dt*dt*invDm*(gradC*gradC.transpose());
        TriDiagonalMatrix Td(A);

        Vector y = Td.solveGaussSeidel(C);
        Vector dx = -dt*dt*invDm*gradC.transpose()*y;
        X = X + dx.transpose();

        QVector<QVector3D> Xt = fromVectorToQVector3D(X);
        C = getC(Xt);

    }

    for(int i = 0; i < X.dim()/3; ++i)
    {
        velocidades[i+1] = (QVector3D(X(3*i+0),X(3*i+1),X(3*i+2)) - posicoes[i+1])/dt;
    }

}

Vector Rod::getC(const QVector<QVector3D>& x)
{
    Vector C( x.size()-1 );
    for(int i = 0; i < C.dim(); ++i)
    {
        C(i) = (x[i+1] - x[i]).lengthSquared()/l[i] -l[i];
    }

    return C;
}

Matrix Rod::getGradC(const QVector<QVector3D> &x)
{
    Matrix gradC(x.size()-1, (x.size()-2)*3);

    QVector3D v = 2*(x[1]-x[0])/l[0];
    gradC(0, 0) = v.x();
    gradC(0, 1) = v.y();
    gradC(0, 2) = v.z();

    for(int i = 1; i < gradC.rows()-1; ++i)
    {
        v = 2*(x[i+1]-x[i])/l[i];
        float j = i-1;
        gradC(i,j*3 + 0) = -v.x();
        gradC(i,j*3 + 1) = -v.y();
        gradC(i,j*3 + 2) = -v.z();
        j = i;
        gradC(i,j*3 + 0) = v.x();
        gradC(i,j*3 + 1) = v.y();
        gradC(i,j*3 + 2) = v.z();
    }
    int i = gradC.rows()-1;
    v = 2*(x[i+1]-x[i])/l[i];
    float j = i-1;
    gradC(i,j*3 + 0) = -v.x();
    gradC(i,j*3 + 1) = -v.y();
    gradC(i,j*3 + 2) = -v.z();

    return gradC;
}
